/*****
  ** @file     : control_config.c/h
  ** @brief    : 控制配置
*****/

//Oneself
#include "control_config.h"
#include "stockpile_f303cc.h"
#include "mt6816.h"
#include "Location_Tracker.h"

/*******************************************电机驱动配置***********************************************/
/*******************************************电机驱动配置***********************************************/
#if MOTOR_DRIVER_PARAM

MotorConfig g_motor_config = {2000,1500,256,50,1000,1000,20,20}; // 声明全局配置实例

// 设置额定电流
ErrStatus setCurrentRatedCurrent(uint16_t _current) 
{
  // 限幅在 0 ~ 2500
  if ((_current > 0) && (_current <= 2500))
  {
    g_motor_config.Rated_Current = _current;
    return SUCCESS;
  }
  else
  {
    return ERROR;
  }
}

// 设置校准电流
ErrStatus setCurrent_Cali_Current(uint16_t _current) 
{
  // 限幅在 0 ~ 2500
  if ((_current > 0) && (_current <= 2500))
  {
    g_motor_config.Cali_Current = _current;
    return SUCCESS;
  }
  else
  {
    return ERROR;
  }
}

// 设置细分
ErrStatus setMoveDivideNum(uint16_t _divide) 
{
  // 限幅在 1 ~ 256
  if ((_divide > 0) && (_divide <= 256))
  {
    g_motor_config.Divide_NUM = _divide;
    return SUCCESS;
  }
  else
  {
    return ERROR;
  }
}

// 设置额定速度
ErrStatus setMoveRatedSpeed(uint16_t _speed) 
{
  // 限幅在 0 ~ 100
  if ((_speed > 0) && (_speed <= 100))
  {
    g_motor_config.Rated_Speed = _speed;
    return SUCCESS;
  }
  else
  {
    return ERROR;
  }
}

// 设置额定加速度
ErrStatus setMoveRatedUpAcc(uint16_t _acc) 
{
  // 限幅在 0 ~ 2000
  if ((_acc > 0) && (_acc <= 2000))
  {
    g_motor_config.Rated_UpAcc = _acc;
    return SUCCESS;
  }
  else
  {
    return ERROR;
  }
}

// 设置额定减速度
ErrStatus setMoveRatedDownAcc(uint16_t _acc) 
{
  // 限幅在 0 ~ 2000
  if ((_acc > 0) && (_acc <= 2000))
  {
    g_motor_config.Rated_DownAcc = _acc;
    return SUCCESS;
  }
  else
  {
    return ERROR;
  }
}

// 设置额定增流梯度
ErrStatus setMoveRatedUpCurrentRate(uint16_t _rate) 
{
  // 限幅在 0 ~ 20
  if ((_rate > 0) && (_rate <= 20))
  {
    g_motor_config.Rated_UpCurrentRate = _rate;
    return SUCCESS;
  }
  else
  {
    return ERROR;
  }
}

// 设置额定减流梯度
ErrStatus setMoveRatedDownCurrentRate(uint16_t _rate)
{
  // 限幅在 0 ~ 20
  if ((_rate > 0) && (_rate <= 20))
  {
    g_motor_config.Rated_DownCurrentRate = _rate;
    return SUCCESS;
  }
  else
  {
    return ERROR;
  }
}

// 获取额定电流
int32_t getCurrentRatedCurrent(void) {
  return g_motor_config.Rated_Current;
}

// 获取校准电流
int32_t getCurrent_Cali_Current(void) {
  return g_motor_config.Cali_Current;
}

// 获取使用的电机单圈步数
// int32_t getMoveStepNum(void) {
//   return g_motor_config.Step_NUM;
// }

// 获取每步柔性件控制细分量
int32_t getMoveDivideNum(void) {
  return g_motor_config.Divide_NUM;
}

// 获取电机单圈脉冲数
int32_t getMovePulseNum(void) {
  return Move_Step_NUM * getMoveDivideNum();
}

// 获取额定转速
int32_t getMoveRatedSpeed(void) {
  return g_motor_config.Rated_Speed * getMovePulseNum();
}

// 获取固件额定加速加速度
int32_t getMoveRatedUpAcc(void) {
  return g_motor_config.Rated_UpAcc * getMovePulseNum();
}

// 获取固件额定减速加速度
int32_t getMoveRatedDownAcc(void) {
  return g_motor_config.Rated_DownCurrentRate * getMovePulseNum();
}

// 获取固件额定增流梯度
int32_t getMoveRatedUpCurrentRate(void) {
  return g_motor_config.Rated_UpCurrentRate * getCurrentRatedCurrent();
}

// 获取固件额定减流梯度
int32_t getMoveRatedDownCurrentRate(void) {
  return g_motor_config.Rated_DownCurrentRate * getCurrentRatedCurrent();
}
#endif


/*******************************************电机控制配置***********************************************/
/*******************************************电机控制配置***********************************************/
#if MOTOR_CONTROL_PARAM 
/*******************  PID控制(速度控制)  **********************/
/*******************  PID控制(速度控制)  ***********************/
//PID控制
Control_PID_Typedef pid;

/**
  * @brief  参数配置
  * @param  _k
  * @retval NULL
**/
ErrStatus Control_PID_SetKP(uint16_t _k)
{
    if(_k <= 1024){        pid.kp = _k;        pid.valid_kp = true;    return SUCCESS;        }
    else{                                      pid.valid_kp = false;   return ERROR;        }
}

/**
  * @brief  参数配置
  * @param  _k
  * @retval NULL
**/
ErrStatus Control_PID_SetKI(uint16_t _k)
{
    if(_k <= 1024){        pid.ki = _k;        pid.valid_ki = true;    return SUCCESS; }
    else{                                      pid.valid_ki = false;    return ERROR;  }
}

/**
  * @brief  参数配置
  * @param  _k
  * @retval NULL
**/
ErrStatus Control_PID_SetKD(uint16_t _k)
{
    if(_k <= 1024){        pid.kd = _k;        pid.valid_kd = true;     return SUCCESS;}
    else{                                      pid.valid_kd = false;  return ERROR; }
}

/**
  * @brief  PID参数恢复
  * @param  NULL
  * @retval NULL
**/
void Control_PID_Set_Default(void)
{
    Control_PID_SetKP(De_PID_KP);
    Control_PID_SetKI(De_PID_KI);
    Control_PID_SetKD(De_PID_KD);
}

/**
  * @brief  控制器PID初始化
  * @param  NULL
  * @retval NULL
**/
void Control_PID_Init(void)
{
    //前置配置无效时,加载默认配置
    if(!pid.valid_kp)                {    Control_PID_SetKP(De_PID_KP);        }
    if(!pid.valid_ki)                {    Control_PID_SetKI(De_PID_KI);        }
    if(!pid.valid_kd)                {    Control_PID_SetKD(De_PID_KD);        }
    
    //控制参数
    pid.v_error = 0;    pid.v_error_last = 0;
    pid.op = 0;                pid.oi = 0;            pid.od = 0;    
    pid.i_mut = 0;        pid.i_dec = 0;
    pid.out = 0;
}

/****************  DCE控制器(位置控制)  *********************/
/****************  DCE控制器(位置控制)  *********************/
//DCE控制
Control_DCE_Typedef dce;

/**
  * @brief  参数配置
  * @param  _k
  * @retval NULL
**/
ErrStatus Control_DCE_SetKP(uint16_t _k)
{
    if(_k <= 1024){        dce.kp = _k;        dce.valid_kp = true;    return SUCCESS;     }
    else{                                      dce.valid_kp = false;  return ERROR;      }
}

/**
  * @brief  参数配置
  * @param  _k
  * @retval NULL
**/
ErrStatus Control_DCE_SetKI(uint16_t _k)
{
    if(_k <= 1024){        dce.ki = _k;        dce.valid_ki = true;   return SUCCESS;     }
    else{                                      dce.valid_ki = false;  return ERROR;      }
}

/**
  * @brief  参数配置
  * @param  _k
  * @retval NULL
**/
ErrStatus Control_DCE_SetKV(uint16_t _k)
{
    if(_k <= 1024){        dce.kv = _k;        dce.valid_kv = true;   return SUCCESS;     }
    else{                                      dce.valid_kv = false;   return ERROR;     }
}

/**
  * @brief  参数配置
  * @param  _k
  * @retval NULL
**/
ErrStatus Control_DCE_SetKD(uint16_t _k)
{
    if(_k <= 1024){        dce.kd = _k;        dce.valid_kd = true;    return SUCCESS;    }
    else{                                      dce.valid_kd = false;   return ERROR;    }
}

/**
  * @brief  DCE参数恢复
  * @param  NULL
  * @retval NULL
**/
void Control_DCE_Set_Default(void)
{
    Control_DCE_SetKP(De_DCE_KP);
    Control_DCE_SetKI(De_DCE_KI);
    Control_DCE_SetKV(De_DCE_KV);
    Control_DCE_SetKD(De_DCE_KD);
}

/**
  * @brief  控制器DCE初始化
  * @param  NULL
  * @retval NULL
**/
void Control_DCE_Init(void)
{
    //前置配置无效时,加载默认配置
    if(!dce.valid_kp)                {    Control_DCE_SetKP(De_DCE_KP);        }
    if(!dce.valid_ki)                {    Control_DCE_SetKI(De_DCE_KI);        }
    if(!dce.valid_kv)                {    Control_DCE_SetKV(De_DCE_KV);        }
    if(!dce.valid_kd)                {    Control_DCE_SetKD(De_DCE_KD);        }

    //控制参数(基本部分)
    dce.p_error = 0;    dce.v_error = 0;
    dce.op = 0;                dce.oi = 0;            dce.od = 0;    
    dce.i_mut = 0;        dce.i_dec = 0;
    dce.out = 0;
}

/*****************  Motor_Control  *****************/
/*****************  Motor_Control  ******************/
Motor_Control_Typedef                 motor_control;                //控制主结构

/**
  * @brief  电机模式配置
  * @param  _mode
  * @retval NULL
**/
void Motor_Control_SetMotorMode(Motor_Mode _mode)
{
    motor_control.mode_order = _mode;
    motor_control.valid_mode = true;
}

/**
  * @brief  堵转保护开关
  * @param  _switch
  * @retval NULL
**/
void Motor_Control_SetStallSwitch(bool _switch)
{
    motor_control.stall_switch = _switch;
    motor_control.valid_stall_switch = true;
}

/**
  * @brief  堵转增流开关
  * @param  _switch
  * @retval NULL
**/
void Motor_Control_SetStallUpRateSwitch(bool _upRateSwitch)
{
    motor_control.stall_UpRate_switch = _upRateSwitch;
    motor_control.valid_stall_UpRate_switch = true;
}

/**
  * @brief  控制模式参数恢复
  * @param  NULL
  * @retval NULL
**/
void Motor_Control_SetDefault(void)
{
    Motor_Control_SetMotorMode(De_Motor_Mode);
    Motor_Control_SetStallSwitch(De_Motor_Stall);
    Motor_Control_SetStallUpRateSwitch(De_Motor_Stall_UpRate);
}

int32_t offset_location = 0;    // 零点偏移位置
int32_t reg_location = 0;       // 通信目标位置

/**
  * @brief  写入目标位置
  * @param  int32_t value
  * @retval NULL
**/
void Motor_Control_Write_Goal_Location(int32_t value)
{
    // reg_location = value;
    // motor_control.goal_location =  reg_location + offset_location;
    motor_control.goal_location =  value;
}
    
/**
  * @brief  写入目标速度
  * @param  int32_t value
  * @retval NULL
**/
void Motor_Control_Write_Goal_Speed(int32_t value)
{
    int32_t temp = Move_Rated_Speed;
    if((value >= -temp) && (value <= temp))
    {
        motor_control.goal_speed = value;
    }
}

/**
  * @brief  写入目标电流
  * @param  uint16_t value
  * @retval NULL
**/
void Motor_Control_Write_Goal_Current(int16_t value)
{
    int32_t temp = Current_Rated_Current;
    if((value >= -temp) && (value <= temp))
    {
        motor_control.goal_current = value;
    }
}

/**
  * @brief  写入目标失能
  * @param  uint16_t value
  * @retval NULL
**/
void Motor_Control_Write_Goal_Disable(uint16_t value)
{
    motor_control.goal_disable = (bool)value;
}

/**
  * @brief  写入目标刹车
  * @param  uint16_t value
  * @retval NULL
**/
void Motor_Control_Write_Goal_Brake(uint16_t value)
{
    motor_control.goal_brake = (bool)value;
}

/* 累计当前脉冲 */
__IO int32_t step_num;

#endif


/**
  * @brief  设置电机ID
  * @param  _id
  * @retval NULL
**/
void Motor_Device_Id_Set(uint8_t _id)
{
    motor_control.device_id = _id;
}

/**
  * @brief  设置零点
  * @param  void
  * @retval NULL
**/
void Motor_Zero_Set(void)
{
  // 修改 motor_control 中的位置变量
    motor_control.real_location = mt6816.rectify_angle;
    motor_control.real_location_last = mt6816.rectify_angle;
    motor_control.est_location = 0;
    motor_control.soft_location = 0;
    motor_control.goal_location = 0;
    
    // 修改 location_tck 中的位置变量
    // location_tck.course_location = 0;
    // location_tck.go_location = 0;
    motor_control.soft_new_curve = true;
    
    // 修改编码器相关位置变量
    motor_control.real_lap_location = mt6816.rectify_angle;
    motor_control.real_lap_location_last = mt6816.rectify_angle;

    step_num = 0;
}

/**
 * @brief  保存电机驱动参数、电机控制参数
 */
ErrStatus Config_Param_save(void)
{
    // 1. 准备待保存的数据（按顺序排列）
    // g_motor_config参数（uint16_t类型，8个）
    uint16_t motor_config_data_s[8] = {
        (uint16_t)g_motor_config.Rated_Current,
        (uint16_t)g_motor_config.Cali_Current,
        (uint16_t)g_motor_config.Divide_NUM,
        (uint16_t)g_motor_config.Rated_Speed,
        (uint16_t)g_motor_config.Rated_UpAcc,
        (uint16_t)g_motor_config.Rated_DownAcc,
        (uint16_t)g_motor_config.Rated_UpCurrentRate,
        (uint16_t)g_motor_config.Rated_DownCurrentRate
    };

    // motor_control参数
    uint16_t device_id_16 = (uint16_t)motor_control.device_id;     // uint8_t转换为16位
    uint16_t mode_order_16 = (uint16_t)motor_control.mode_order;   // 电机运行模式枚举转换为16位
    uint16_t stall_switch_16 = (uint16_t)motor_control.stall_switch; // 堵转保护开关bool转换为16位
    uint16_t stall_UpRate_switch_16 = (uint16_t)motor_control.stall_UpRate_switch; // 堵转增流开关bool转换为16位

    // dce参数（int32_t类型，4个）
    uint16_t dce_data[4] = {
        (uint16_t)dce.kp,
        (uint16_t)dce.ki,
        (uint16_t)dce.kv,
        (uint16_t)dce.kd
    };

    // pid参数（int32_t类型，3个）
    uint16_t pid_data[3] = {
        (uint16_t)pid.kp,
        (uint16_t)pid.ki,
        (uint16_t)pid.kd
    };

    // 2. 准备Flash写入
    Stockpile_Flash_Data_Empty(&stockpile_data);        //擦除数据区
    Stockpile_Flash_Data_Begin(&stockpile_data);        //开始写数据区

    // 3. 写入g_motor_config参数（16位数据，8个半字）
    Stockpile_Flash_Data_Write_Data16(&stockpile_data, motor_config_data_s, 8);

    // 4. 写入motor_control.device_id（uint8_t → 16位，低8位有效）
    Stockpile_Flash_Data_Write_Data16(&stockpile_data, &device_id_16, 1);

    // 5. 写入motor_control.mode_order（枚举 → 16位）
    Stockpile_Flash_Data_Write_Data16(&stockpile_data, &mode_order_16, 1);

    // 6. 写入motor_control.stall_switch（bool → 16位）
    Stockpile_Flash_Data_Write_Data16(&stockpile_data, &stall_switch_16, 1);

    // 7. 写入motor_control.stall_UpRate_switch（bool → 16位）
    Stockpile_Flash_Data_Write_Data16(&stockpile_data, &stall_UpRate_switch_16, 1);

    // 8. 写入dce参数（4个16位数据）
    Stockpile_Flash_Data_Write_Data16(&stockpile_data, dce_data, 4);

    // 9. 写入pid参数（3个16位数据）
    Stockpile_Flash_Data_Write_Data16(&stockpile_data, pid_data, 3);

    // 10. 结束写入并锁定Flash
    Stockpile_Flash_Data_End(&stockpile_data);

    for (uint8_t i = 0; i < CONFIG_PARAM_SAVE_NUM; i++)
    {
        if (Read_MOTOR_PARAM_DATA[i] == 0xFFFF)
        {
            return ERROR;
        }
    }
    return SUCCESS;
}

//编码器矫正数据位置(Quick)(Flash)
uint16_t *Read_MOTOR_PARAM_DATA = (uint16_t*)STOCKPILE_APP_DATA_ADDR;

/**
 * @brief  读取电机驱动参数、电机控制参数
 */
void Config_Param_Read(void)
{
    uint16_t * ptr = (uint16_t *)&g_motor_config;
    uint8_t i = 0;
    for (i = 0; i < 8; i++)
    {
        ptr[i] = Read_MOTOR_PARAM_DATA[i];
    }

    motor_control.device_id = (uint8_t)Read_MOTOR_PARAM_DATA[i++];
    motor_control.mode_order = (Motor_Mode)Read_MOTOR_PARAM_DATA[i++];
    motor_control.stall_switch = (uint8_t)Read_MOTOR_PARAM_DATA[i++];
    motor_control.stall_UpRate_switch = (uint8_t)Read_MOTOR_PARAM_DATA[i++];
    dce.kp = Read_MOTOR_PARAM_DATA[i++];
    dce.ki = Read_MOTOR_PARAM_DATA[i++];
    dce.kv = Read_MOTOR_PARAM_DATA[i++];
    dce.kd = Read_MOTOR_PARAM_DATA[i++];
    pid.kp = Read_MOTOR_PARAM_DATA[i++];
    pid.ki = Read_MOTOR_PARAM_DATA[i++];
    pid.kd = Read_MOTOR_PARAM_DATA[i++];
}

